/*************************************************
 *  ITG3200 Library
 *************************************************/

#include "lib_itg3200.h"

void ITG3200_init() {
    unsigned short temp;

    // Set clk_sel on Power Management register (Set Gyro-Z clock)
    I2C1_Start();
    I2C1_Wr(ITG3200_WRITE_ADDR);      // Write mode
    I2C1_Wr(0x3E);
    I2C1_Stop();
    I2C1_Start();
    I2C1_Wr(ITG3200_READ_ADDR);       // Read Mode
    temp = I2C1_Rd(0);
    I2C1_Stop();
    I2C1_Start();
    I2C1_Wr(ITG3200_WRITE_ADDR);      // Write mode
    I2C1_Wr(0x3E);
    I2C1_Wr(((temp & 0xF1) | 0x03));
    I2C1_Stop();
    Delay_ms(50);

    // Set smplrt_div on Sample Rate Divider register (Set output at 500Hz rate)
    I2C1_Start();
    I2C1_Wr(ITG3200_WRITE_ADDR);      // Write mode
    I2C1_Wr(0x15);
    I2C1_Wr(0x01);
    I2C1_Stop();
    Delay_ms(10);

    // Set fs_sel and dlpf_cfg on Full scale register (Set lowpass filter to 10Hz value)
    I2C1_Start();
    I2C1_Wr(ITG3200_WRITE_ADDR);      // Write mode
    I2C1_Wr(0x16);
    I2C1_Wr(0x1D);
    I2C1_Stop();
    Delay_ms(10);
}

void ITG3200_readGyro(float *gyro) {
    unsigned short cnt;
    unsigned short gyro_read[6];
    int gyro_int[3];

    I2C1_Start();
    I2C1_Wr(ITG3200_WRITE_ADDR);      // Write mode
    I2C1_Wr(0x1D);
    I2C1_Stop();
    I2C1_Start();
    I2C1_Wr(ITG3200_READ_ADDR);       // Read Mode
    for (cnt=0; cnt<6; cnt++) {
        char ack;
        ack = (cnt<5);
        gyro_read[cnt] = I2C1_Rd(ack);
    }
    I2C1_Stop();
    
    gyro_int[0] = gyro_read[0];
    gyro_int[0] = gyro_int[0] << 8;
    gyro_int[0] |= gyro_read[1];
    gyro_int[1] = gyro_read[2];
    gyro_int[1] = gyro_int[1] << 8;
    gyro_int[1] |= gyro_read[3];
    gyro_int[2] = gyro_read[4];
    gyro_int[2] = gyro_int[2] << 8;
    gyro_int[2] |= gyro_read[5];
    
    gyro[0] = (float)(gyro_int[0] / 14.375);
    gyro[1] = (float)(gyro_int[1] / 14.375);
    gyro[2] = (float)(gyro_int[2] / 14.375);
}